import sys
import time

import ecal.core.core as ecal_core
from ecal.core.publisher import ProtoPublisher

# Import the "hello_world_pb2.py" file that we have just generated from the
# proto_messages directory 
import proto_messages.hello_world_pb2 as hello_world_pb2
import serial
import re

utctime = ''
lat = ''
ulat = ''
lon = ''
ulon = ''
numSv = ''
msl = ''
cogt = ''
cogm = ''
sog = ''
kph = ''
gps_t = 0

ser = serial.Serial("/dev/ttyACM0", 115200)

if ser.isOpen():
    print("GPS Serial Opened! Baudrate=115200")
else:
    print("GPS Serial Open Failed!")


def Convert_to_degrees(in_data1, in_data2):
    len_data1 = len(in_data1)
    str_data2 = "%05d" % int(in_data2)
    temp_data = int(in_data1)
    symbol = 1
    if temp_data < 0:
        symbol = -1
    degree = int(temp_data / 100.0)
    str_decimal = str(in_data1[len_data1-2]) + str(in_data1[len_data1-1]) + str(str_data2)
    f_degree = int(str_decimal)/60.0/100000.0
    # print("f_degree:", f_degree)
    if symbol > 0:
        result = degree + f_degree
    else:
        result = degree - f_degree
    
    return result



def GPS_read(utctime,lat,ulat,lon,ulon,numSv,msl,cogt,cogm,sog,kph,gps_t):
	
	#GGA = ser.read(700)#GNGGA,064839.00,3111.27952,N,12034.98476
	#print('GGA',GGA)
	if ser.inWaiting():
		print('ser.inWaiting()')
		GGA = ser.read(ser.inWaiting())#GNGGA,064839.00,3111.27952,N,12034.98476
		print('GGA',GGA)
		GGA_g = re.findall(r"\w+(?=,)|(?<=,)\w+", str(GGA))
		print('GGA_gd',GGA_g)
		if GGA_g[0]=='GNRMC':
			utctime = GGA_g[1]
			# lat = GGA_g[2][0]+GGA_g[2][1]+'°'+GGA_g[2][2]+GGA_g[2][3]+'.'+GGA_g[3]+'\''
			#lat = "%.8f" % Convert_to_degrees(str(GGA_g[4]), str(GGA_g[5]))
			lat= Convert_to_degrees(str(GGA_g[4]), str(GGA_g[5]))
			
			ulat = GGA_g[6]
			# lon = GGA_g[5][0]+GGA_g[5][1]+GGA_g[5][2]+'°'+GGA_g[5][3]+GGA_g[5][4]+'.'+GGA_g[6]+'\''
			lon = Convert_to_degrees(str(GGA_g[7]), str(GGA_g[8]))
			ulon = GGA_g[9]
			numSv = ' '
			msl = GGA_g[12]+'.'+GGA_g[13]+GGA_g[14]
			#print(GGA_g)
			gps_t = 1
			return utctime,lat,ulat,lon,ulon,numSv,msl,cogt,cogm,sog,kph,gps_t
		
	return utctime, lat, ulat, lon, ulon, numSv, msl, cogt, cogm, sog, kph, gps_t
if __name__ == "__main__":
	# initialize eCAL API. The name of our Process will be
	# "Python Protobuf Publisher"
	ecal_core.initialize(sys.argv, "Python Protobuf Publisher")
	
	# Create a Protobuf Publisher that publishes on the topic
	# "hello_world_python_protobuf_topic". We also pass it the datatype we are
	# going to send later. By doing this we enable the eCAL Monitor to dynamically
	# show the content of the messages.
	pub = ProtoPublisher("hello_world_python_protobuf_topic"
                    	, hello_world_pb2.HelloWorld)
	
	# Create a counter and some messages, so we see something changing in the
	# message, later
	counter = 0
	dummy_messages = ["The story so far:"
                	, "In the beginning the Universe was created."
                	, "This has made a lot of people very angry"
                	, "and been widely regarded as a bad move."]
	
	# Infinite loop (using ecal_core.ok() will enable us to gracefully shutdown
	# the process from another application)
	while ecal_core.ok():
		utctime,lat,ulat,lon,ulon,numSv,msl,cogt,cogm,sog,kph,gps_t=GPS_read(utctime,lat,ulat,lon,ulon,numSv,msl,cogt,cogm,sog,kph,gps_t)
		
		# Create a message and fill it with some data
		protobuf_message = hello_world_pb2.HelloWorld()
		protobuf_message.name = "Douglas Adams"
		if lat!='':
			protobuf_message.id   = int(lat)
		protobuf_message.msg  = dummy_messages[counter % 4]
		
		print("Sending message {}".format(counter))
		
		# actually send the message to the topic this publisher was created for
		pub.send(protobuf_message)
		
		# Sleep 1s
		time.sleep(1)
		
		counter = counter + 1
		
		# finalize eCAL API
	ecal_core.finalize()
